package ncit.bluetooth.control;

import java.util.Calendar;

import ncit.bluetooth.robots.*;
import ncit.bluetooth.settings.BluetoothControlSettings;
import android.os.Handler;
import android.os.Message;
import android.os.PowerManager;
import android.util.Log;

public class WorkingThread extends Thread {

	/**
	 * Constants used for Debugging.
	 */
	private static final String TAG = "WorkingThread";

	/**
	 * Constants used for Handler.
	 */
	public static final int MESSAGE_START = 1;
	public static final int MESSAGE_FORWARD = 2;
	public static final int MESSAGE_LEFT = 3;
	public static final int MESSAGE_BREAK = 4;
	public static final int MESSAGE_ASK_SENSORS = 5;
	public static final int MESSAGE_RECV = 6;
	public static final int MESSAGE_STOP = 7;
	public static final int MESSAGE_ROBOT_CHANGE = 8;
	public static final int MESSAGE_PARK = 9;

	
	/**
	 * The robot/device this application will control.
	 */
	private Robot myRobot = null;
	
	/**
	 * Calendar instance used to calculate time.
	 */
	private Calendar calendar = Calendar.getInstance();
	
	/**
	 * Used to calculate offset between commands.
	 */
	private long lastGoForwardAction = calendar.getTimeInMillis();
	
	/**
	 * Used to calculate offset betweeen commands.
	 */
	private long lastGoLeftAction = calendar.getTimeInMillis();
	
	/**
	 * Used to calculate offset between commands.
	 */
	private long crAction;

	/**
	 * A UIThread Handler used to post message to the UI thread.
	 */
	private Handler uiThreadHandler;
	
	/**
	 * Variable used to stop the thread.
	 */
	private boolean working;

	/**
	 * Variable used to make the screen not sleep while the application is running.
	 */
	PowerManager.WakeLock wl = null;

	/**
	 * A time limit/offset in order to diferentiate commands.
	 */
	public long limit;

	/**
	 * This thread's handler. Used to pass messages between the UI Thread, the Accelerometer
	 * Listener and the Robot Device.
	 */
	private Handler controlsHandler = new Handler() {

		@Override
		public void handleMessage(Message msg) {

			if (!working) {

				/**
				 * Instantiate the appropiate robot.
				 */
				if (msg.what == MESSAGE_ROBOT_CHANGE){
					
					if (msg.arg1 == 2){
						myRobot = new LineFollower();
						limit = -1;
					}
					else if (msg.arg1 == 0)
						myRobot = new NXTLego();
					else if (msg.arg1 == 1)
						myRobot = new GenericBluetoothDevice();
					
				}else if(msg.what == MESSAGE_START){

					if (BluetoothControlActivity.DEBUG)
						Log.i(TAG, "MESSAGE_START: ");

					myRobot.startMotors();
					myRobot.startSensors();
					startWorking();
					
				}
				
				
				return;
			}

			switch (msg.what) {
			
			case MESSAGE_START: {
				if (BluetoothControlActivity.DEBUG)
					Log.i(TAG, "MESSAGE_START: ");

				myRobot.startMotors();
				myRobot.startSensors();

				break;
			}
			case MESSAGE_FORWARD: {

				calendar = Calendar.getInstance();
				crAction = calendar.getTimeInMillis();

				if (BluetoothControlActivity.DEBUG)
					Log.i(TAG, "MESSAGE_FORWARD: " + msg.obj + " " + crAction
							+ " " + lastGoForwardAction);
				
				if (crAction - lastGoForwardAction > limit) {

					myRobot.goForwardBackward((Float) msg.obj);
					lastGoForwardAction = crAction;

					// myRobot.goForward = ((Float)msg.obj < 0);
				}
				break;
			}
			case MESSAGE_LEFT: {
				if (BluetoothControlActivity.DEBUG)
					Log.i(TAG, "MESSAGE_LEFT: " + msg.obj);
				
				calendar = Calendar.getInstance();
				crAction = calendar.getTimeInMillis();

				if (crAction - lastGoLeftAction > limit) {

					float slope = (Float) msg.obj;
					myRobot.goLeftRight(slope);
					lastGoLeftAction = crAction;

				}
				break;
			}
			case MESSAGE_BREAK: {

				myRobot.motorBreak();
				break;
			}
			case MESSAGE_ASK_SENSORS: {

				if (BluetoothControlActivity.DEBUG)
					Log.d(TAG, "MESSAGE_ASK_SENSORS ");

				myRobot.askSensors();
				break;

			}
			case MESSAGE_RECV: {

				if (BluetoothControlActivity.DEBUG)
					Log.d(TAG, "MESSAGE_RECV ");

				byte[] data = (byte[]) msg.obj;
//				int distance = 0;

				if (data[3] == 0x07) {

					Message msg1 = uiThreadHandler
							.obtainMessage(BluetoothControlActivity.MESSAGE_WITH_WORKINGTHREAD_HANDLER);
					
					/*TODO: for future developement.
					 * if (data[5] == Commands.ULTRASOUND_SENSOR) {

						if ((data[7]) == 0) {
							distance = ((int) (data[12]) << 4) + (int) data[13];
							
							Log.i("wthr_recv", "Next obstacle in "
									+ (int) (distance * 2.5) + " cm........."
									+ new Byte(data[13]).intValue() + " "
									+ new Byte(data[12]).intValue());
							
						} else {
							distance = ((int) (data[10]) << 4) + (int) data[11]; // between
																					// 0-1023
							distance /= 41;
							Log.i("wthr_recv", "Next obstacle in "
									+ (int) (distance * 2.5) + " cm........."
									+ new Byte(data[10]).intValue() + " "
									+ new Byte(data[11]).intValue());

							Log.i(TAG, "Next obstacle in " + distance + " cm ");
						}
						msg1.arg1 = Commands.ULTRASOUND_SENSOR;
						msg1.arg2 = distance;

					} else if (data[5] == Commands.LEFT_BACK_TOUCH_SENSOR) {
						msg1.arg1 = Commands.LEFT_BACK_TOUCH_SENSOR;
						msg1.arg2 = data[14];
					} else if (data[5] == Commands.RIGHT_BACK_TOUCH_SENSOR) {
						msg1.arg1 = Commands.RIGHT_BACK_TOUCH_SENSOR;
						msg1.arg2 = data[14];
					}
*/
					msg1.sendToTarget();
				}
				break;
			}
			case MESSAGE_STOP: {
				
				if (BluetoothControlActivity.DEBUG)
					Log.i(TAG, "MESSAGE_STOP: ");

				myRobot.motorBreak();

				break;
			}
			case MESSAGE_ROBOT_CHANGE: {

				if(BluetoothControlActivity.DEBUG)
					Log.d(TAG, "MESSAGE_ROBOT_CHANGE: ");

				if (msg.arg1 == 1) {

					
				} else if (msg.arg1 == 2) {

					myRobot = new LineFollower();
					limit = 0;
					
				} else if(msg.arg1 == 3){
					
					myRobot = new GenericBluetoothDevice();
					limit = 0;
					
				}
				
				if(BluetoothControlActivity.DEBUG)
					Log.d(TAG, "The new device is of type: " +  myRobot.getClass().getName());
				break;
			}

			case MESSAGE_PARK: {
				if (BluetoothControlActivity.DEBUG)
					Log.i(TAG, "MESSAGE_PARK: ");

				myRobot.park();

				break;
			}
			}
		}
	};

	/**
     * 
     */
	public WorkingThread(Handler UIThreadHandler) {
		
		// set the handler to the UIThread. Used for sending view data
		this.uiThreadHandler = UIThreadHandler;

		// instantiate the robot control class
		//myRobot = new NXTLego(this.controlsHandler);

		if (BluetoothControlSettings.DEVICE_TYPE == 2){
			myRobot = new LineFollower();
			limit = -1;
		}
		
		else if (BluetoothControlSettings.DEVICE_TYPE == 0)
			myRobot = new NXTLego();
		
		else if (BluetoothControlSettings.DEVICE_TYPE == 1)
			myRobot = new GenericBluetoothDevice();
		
		
		// set the working variable to true
		//this.working = false;
		lastGoForwardAction = calendar.getTimeInMillis();
		lastGoLeftAction = calendar.getTimeInMillis();
		limit = 1000; //time offset between commands

	}

	/**
	 * Run method of this class.
	 * Enters a loop where it constantly ask for sensor data.
	 * TODO: implement correct sensor reading for future versions.
	 */
	@Override
	public void run() {

		PowerManager.WakeLock wl = BluetoothControlActivity.powerManager
				.newWakeLock(PowerManager.SCREEN_DIM_WAKE_LOCK, "My Tag");
		wl.acquire();

		this.working = true;
		
		// here we begin
		while (this.working) {

			try {

				if(myRobot instanceof LineFollower){
					
					((LineFollower)myRobot).sendMessage();
					
					continue;
				}
				
				Thread.sleep(2000);
				Message msg = controlsHandler
						.obtainMessage(MESSAGE_ASK_SENSORS);
				msg.sendToTarget();

			} catch (InterruptedException e) {

				this.working = false;
				e.printStackTrace();

			} finally {

			}

		}

		if (wl != null)
			wl.release();

	}

	public Handler getHandler() {
		return controlsHandler;
	}

	public void startWorking() {
		this.working = true;
	}

	public void stopWorking() {
		this.working = false;
	}

	public synchronized void stopAll() {
		this.stopWorking();
		this.stop();
	}

}
